/**
  ******************************************************************************
  * Copyright (C) 2020 - ~, SCUT-RobotLab Development Team
  * @file    state_Locomotion.cpp
  * @author  Mentos Seetoo
  * @brief   Finite State Machine for standing up.
  * @note    
  * @warning 
  *     - At least C++11 is required.														 
  ******************************************************************************
  */
#include "state_Locomotion.h"
#include "matplotlibcpp.h"
/* Private defines -----------------------------------------------------------*/
using namespace Quadruped;
using namespace std;

/* Founctions ----------------------------------------------------------------*/
template <typename T>
FSM_Locomotion<T>::FSM_Locomotion(GaitScheduler<T> *p_gait,
                StateEstimator<T> *p_estimator,
                DesiredCoMState<T> *p_command,
                Dynamic<T> *p_dynamic,
                BalanceController<T>  *p_controller,
                T run_period):FSM_Base<T>::FSM_Base(run_period)
{
    FSM_Base<T>::_name = FSM_StateName::LOCOMOTION;
    p_BalanceController = p_controller;
    p_Dynamic = p_dynamic;
    p_GaitScheduler = p_gait;
    p_CoMCommand = p_command;
    p_StateEstimator = p_estimator;
}

template <typename T>
void FSM_Locomotion<T>::RunState()
{
    /*
        Motion Scheduler: Foot placement in swing phase & stance phase.
    */
    Vec3<T> pNextStep[4],pBackPoint;
    Vec3<T> pDesiredFoot[4] = {Vec3<T>::Zero()};

    p_CoMCommand->v_d =p_GaitScheduler->GetVBody();
    for(int leg(0); leg < 4; leg++)
    {
        //!< Raibert heuristic and velocity-based feedback term
        //!< from capture point formulation.
        pNextStep[leg] = p_Dynamic->BodyState.rBody_M2B * (p_Dynamic->getHipLocation(leg) + getLegSign(leg) * p_Dynamic->_hipLocation) - Vec3<T>(0,0,p_CoMCommand->CoM_height)
                    +  0.5f * p_GaitScheduler->GetPeriod() * p_CoMCommand->v_d
                    //+  sqrt(p_CoMCommand->CoM_height/ CONST_GRAVITY) * (p_StateEstimator->v - p_CoMCommand->v_d)
                    +  0.5f * sqrt(p_CoMCommand->CoM_height/ CONST_GRAVITY) * p_StateEstimator->v.cross(p_CoMCommand->w_d);
        
        p_GaitScheduler->SetFootPlacement(p_StateEstimator->p_foot_M[leg], pNextStep[leg], 0.04, leg);                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                           
        SysLog->Record(_INFO_,"Scheduler","Swing  Leg %d next: %f, %f, %f",leg,
                        pNextStep[leg](0), pNextStep[leg](1), pNextStep[leg](2));
    }

    //!< Generate trajectory in {M}.
    p_GaitScheduler->gaitGenerate();

    /*
        Balance controller: Stabilize CoM velocity and rpy.
    */
    //!< update targets and feedback values
    p_BalanceController->SetTargets(p_CoMCommand->p_d, p_CoMCommand->v_d, p_CoMCommand->w_d,
                                    ori::rpyToRotMat(p_CoMCommand->rpy_d));
    p_BalanceController->UpdateValues(p_StateEstimator->p, p_StateEstimator->v, p_StateEstimator->w,
                                    ori::rpyToRotMat(p_StateEstimator->rpy));
    //!< Calculate controller output.
    p_BalanceController->CalConrtollerOutput();
    //RotMat<T> _rBody_Ctrl = ori::rpyToRotMat(p_BalanceController->out_w);
    
    SysLog->Record(_WARN_,"Controller","Position error: %f, %f, %f",p_BalanceController->p_error(0),
                    p_BalanceController->p_error(1),p_BalanceController->p_error(2));
    SysLog->Record(_WARN_,"Controller","Postion Adjust: %f, %f, %f",p_BalanceController->out_v(0),
                    p_BalanceController->out_v(1),p_BalanceController->out_v(2));
    SysLog->Record(_WARN_,"Controller","Ori error: %f, %f, %f",p_BalanceController->ori_error(0),
                    p_BalanceController->ori_error(1),p_BalanceController->ori_error(2));
    SysLog->Record(_WARN_,"Controller","Ori Adjust: %f, %f, %f",p_BalanceController->out_w(0),
                    p_BalanceController->out_w(1),p_BalanceController->out_w(2));
    /*
        Set targets in {B}
    */
    Vec3<T> ctrl_pos = Vec3<T>::Zero();
    for (uint8_t leg(0); leg < 4; leg++)
    {
        //!< Swing leg
        if(p_GaitScheduler->GetFootState(leg) == ContactState::swingState)
        {
            pDesiredFoot[leg] = p_GaitScheduler->GetFootPos(leg);
        }
        //!< Stance leg
        else
        {
            // //!< Using controller output
            // pDesiredFoot[leg] = p_StateEstimator->p_foot_G[leg] - p_BalanceController->out_p;
            // //ctrl_pos = _rBody_Ctrl.transpose()*pDesiredFoot[leg] - p_Dynamic->getHipLocation(leg);
            // p_Dynamic->LegCommand[leg].p = _rBody_Ctrl.transpose()*pDesiredFoot[leg] - p_Dynamic->getHipLocation(leg);

            // SysLog->Record(_WARN_,"Controller","Close loop Leg Command in M-Frame %d: %f, %f, %f",leg,
            //                 pDesiredFoot[leg](0),
            //                 pDesiredFoot[leg](1), 
            //                 pDesiredFoot[leg](2));
            // SysLog->Record(_WARN_,"Controller","Close loop Leg Command in B-Frame %d: %f, %f, %f",leg,
            //                 ctrl_pos(0), ctrl_pos(1), ctrl_pos(2));
            Vec3<T> delta_v = p_BalanceController->out_w.cross(p_Dynamic->getHipLocation(leg)) +
                              p_BalanceController->out_v;

            //!< cal foot location in {M}
            pDesiredFoot[leg] = p_Dynamic->BodyState.rBody_M2B * 
                                (p_Dynamic->LegCommand[leg].p + p_Dynamic->getHipLocation(leg)) 
                               + p_CoMCommand->dt * (-p_CoMCommand->v_d + delta_v);
            SysLog->Record(_WARN_,"Scheduler","Open loop Leg Command in M-Frame %d: %f, %f, %f",leg,
                            pDesiredFoot[leg](0),
                            pDesiredFoot[leg](1), 
                            pDesiredFoot[leg](2));
        }

        p_Dynamic->LegCommand[leg].p = (p_Dynamic->BodyState.rBody_M2B.transpose() * pDesiredFoot[leg])
                              - p_Dynamic->getHipLocation(leg);

        SysLog->Record(_WARN_,"Scheduler","Open loop Leg Command in B-Frame %d: %f, %f, %f",leg,
                        p_Dynamic->LegCommand[leg].p(0), p_Dynamic->LegCommand[leg].p(1), p_Dynamic->LegCommand[leg].p(2));
    }
}

/**
* @brief  
* @return 
* @author Mentos Seetoo
*/
template <typename T>
int FSM_Locomotion<T>::Transition(FSM_Base<T>* target_state)
{

}


template class FSM_Locomotion<float>;
template class FSM_Locomotion<double>;
/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/